#include <stdio.h>
#include <stdlib.h>

#include <webots/motor.h>
#include <webots/robot.h>
#include <webots/gps.h>
#include <webots/position_sensor.h>

#include "gait.h"
#include "movebase.h"

#define RPM_TO_RADPS(value) (value / 9.549f)
#define RADPS_TO_RPM(value) (value * 9.549f)

#define DEG_TO_RPM(value) (value / 57.3f)
#define RPM_TO_DEG(value) (value * 57.3f)

#define TIME_STEP 10     // ms
#define TIME_STEP_S 0.01 // s
// #define TIME_STEP 5     // ms
// #define TIME_STEP_S 0.005 // s

extern GaitState_e GAIT_STATE; // 状态机索引

double ps1_1_spd = 0;
double ps1_1_pos = 0;
double ps1_1_pos_last = 0;

double gps_height = 0;
double gps_speed = 0;

double foot_gps_height = 0;

double motor1_1_torque = 0;

int main(int argc, char **argv)
{
  wb_robot_init();
  printf("*************** CONTROLLER START!!! *************** \r\n");

  // 打开文件以写入数据
  FILE *file = fopen("data.dat", "w");
  if (file == NULL)
  {
    printf("Error opening file!\n");
    wb_robot_cleanup();
    return 1;
  }

  WbDeviceTag foot_gps = wb_robot_get_device("gps1");
  wb_gps_enable(foot_gps, TIME_STEP);

  // 获取 GPS 设备
  WbDeviceTag gps = wb_robot_get_device("gps");
  wb_gps_enable(gps, TIME_STEP);

  WbDeviceTag motor1_1 = wb_robot_get_device("motor1_1");
  WbDeviceTag motor1_2 = wb_robot_get_device("motor1_2");
  WbDeviceTag motor2_1 = wb_robot_get_device("motor2_1");
  WbDeviceTag motor2_2 = wb_robot_get_device("motor2_2");

  wb_motor_enable_torque_feedback(motor1_1, TIME_STEP);
  wb_motor_enable_torque_feedback(motor1_2, TIME_STEP);
  wb_motor_enable_torque_feedback(motor2_1, TIME_STEP);
  wb_motor_enable_torque_feedback(motor2_2, TIME_STEP);

  float max_rpm = 300;
  wb_motor_set_velocity(motor1_1, RPM_TO_RADPS(max_rpm)); // rad/s (x9.549)
  wb_motor_set_velocity(motor1_2, RPM_TO_RADPS(max_rpm));
  wb_motor_set_velocity(motor2_1, RPM_TO_RADPS(max_rpm));
  wb_motor_set_velocity(motor2_2, RPM_TO_RADPS(max_rpm));

  float max_torque = 100;
  wb_motor_set_available_torque(motor1_1, max_torque); // n.m
  wb_motor_set_available_torque(motor1_2, max_torque);
  wb_motor_set_available_torque(motor2_1, max_torque);
  wb_motor_set_available_torque(motor2_2, max_torque);

  // 获取电机位置传感器
  WbDeviceTag ps1_1 = wb_robot_get_device("motor1_1 sensor");
  // WbDeviceTag ps1_2 = wb_robot_get_device("motor1_2 sensor");
  // WbDeviceTag ps2_1 = wb_robot_get_device("motor2_1 sensor");
  // WbDeviceTag ps2_2 = wb_robot_get_device("motor2_2 sensor");
  wb_position_sensor_enable(ps1_1, TIME_STEP);
  // wb_position_sensor_enable(ps1_2, TIME_STEP);
  // wb_position_sensor_enable(ps2_1, TIME_STEP);
  // wb_position_sensor_enable(ps2_2, TIME_STEP);

  while (wb_robot_step(TIME_STEP) != -1)
  {

    const double *foot_gps_pos = wb_gps_get_values(foot_gps);
    foot_gps_height = foot_gps_pos[1];

    const double *gps_pos = wb_gps_get_values(gps); // [0]X [1]Y [2]Z M
    const double gps_spd = wb_gps_get_speed(gps);   // M/S
    gps_height = gps_pos[1];
    gps_speed = gps_spd;

    ps1_1_pos = wb_position_sensor_get_value(ps1_1);        // rad
    ps1_1_spd = (ps1_1_pos - ps1_1_pos_last) / TIME_STEP_S; // rad/s
    ps1_1_pos_last = ps1_1_pos;

    motor1_1_torque = wb_motor_get_torque_feedback(motor1_1);

    // printf("GPS 坐标：(%f, %f, %f)\n", values[0], values[1], values[2]); //values[1] 为向上
    // 写入数据到文件
    // fprintf(file, "%f %f\n", wb_motor_get_force_feedback(motor1_1), wb_motor_get_torque_feedback(motor1_1));
    // fprintf(file, "%f %f\n", wb_motor_get_torque_feedback(motor1_1), wb_motor_get_torque_feedback(motor1_2));

    // fprintf(file, "%f %f %f\n", wb_motor_get_torque_feedback(motor1_1), wb_motor_get_velocity(motor1_1), wb_motor_get_acceleration(motor1_1));
    // fprintf(file, "%f %f\n", wb_motor_get_torque_feedback(motor1_1), values[1]);
    // fprintf(file, "%f %f\n", wb_motor_get_torque_feedback(motor1_1), wb_motor_get_torque_feedback(motor1_2), values[1]);

    // fprintf(file, "%f %f %f %f\n", wb_motor_get_torque_feedback(motor1_1), wb_motor_get_torque_feedback(motor1_2), wb_motor_get_torque_feedback(motor2_1), wb_motor_get_torque_feedback(motor2_2));

    // fprintf(file, "%f\n", ps11_pos);

    // printf("wb_motor_get_velocity %f\n", wb_motor_get_velocity(motor1_1));
    // printf("ps11_pos %f\n", ps11_pos);

    // fprintf(file, "%f %f %f %f\n", wb_motor_get_torque_feedback(motor1_1), wb_motor_get_acceleration(motor1_1), wb_motor_get_velocity(motor1_1), values[1]);

    // fprintf(file, "%f %f %f %f %f %f\n", motor1_1_torque, RADPS_TO_RPM(ps1_1_spd), RPM_TO_DEG(ps1_1_pos), gps_height, foot_gps_height, gps_speed);
    fprintf(file, "%f %f %f %f %f\n", motor1_1_torque, RADPS_TO_RPM(ps1_1_spd), RPM_TO_DEG(ps1_1_pos), gps_height, foot_gps_height-0.07);

    GaitTask();
    //  Output_Wheel(0.0f, 0.2f, 0.0f);

    if (GAIT_STATE == STOP)
    {
      break;
    }
  };

  // 关闭文件
  fclose(file);

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  // 调用系统命令来执行Python脚本
  printf("plot \r\n");
  // system("python plot.py");

  return 0;
}
